SE3.h

Go to the documentation of this file.

// WOLF - Copyright (C) 2020-2026
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
 
#include "core/common/wolf.h"
#include "core/math/rotations.h"
#include "core/composite/vector_composite.h"
 
/*
 * The functions in this file are related to manipulations of Delta motion magnitudes used in 3d motion.
 *
 * The Delta is defined as a simple 3d pose with the rotation expressed in quaternion form,
 *     Delta = [Dp, Dq]
 * with
 *     Dp : position delta
 *     Dq : quaternion delta
 * and its tangent vector (Lie algebra) is tau = [rho, theta].
 *
 * The functions are listed below:
 *
 *   - identity:    I  = Delta at the origin, with Dp = [0,0,0]; Dq = [0,0,0,1], so that D (+) I = I (+) D = D
 *   - inverse:     so that D (+) D.inv = D.inv (+) D = I
 *   - compose:     Dc = D1 (+) D2
 *   - between:     Db = D2 (-) D1 := D1.inv (+) D2, so that D2 = D1 (+) Db
 *   - exp:         go from tangent space to delta manifold (SE(3) exponential map)
 *   - log:         go from delta manifold to tangent space (SE(3) logarithmic map)
 *   - plus:        block-wise: D2 = [Dp1 + dp, Dq1 * exp_q(do)]
 *   - minus:       block-wise: d  = [Dp2 - Dp1, log_q(Dq1.conj * Dq2)], so that D2 = plus(D1, d)
 *   - interpolate: D  = D1 (+) exp( t * log( D1.inv (+) D2 ) )
 *   - toSE3:       4x4 homogeneous matrix
 *   - jac_SE3_left, jac_SE3_right, jac_SE3_left_inv, jac_SE3_right_inv: Jacobians of SE(3)
 *
 * Each function is provided (when applicable) with the following APIs:
 *   - by parts, with the rotation as a quaternion (Eigen::QuaternionBase)
 *   - by parts, with the rotation as a vector (quaternion coefficients [x, y, z, w])
 *   - full 7-vector [p, q] as output parameter and as return value
 *   - full 7-vector with Jacobians. Jacobians are block-wise: w.r.t. perturbations [dp, do] with additive dp
 *     and local (right) do on the quaternion, i.e. consistent with plus() and minus().
 *   - VectorComposite with keys 'P' and 'O', as output parameter and as return value
 *
 * ------------------------------------------------------------------------------------------
 *
 * Some of the functions below are based on:
 *
 * [1] J. Sola et. al. "A micro Lie theory for state estimation in robotics", tech rep. IRI 2018,
 * https://arxiv.org/pdf/1812.01537.pdf
 */
 
namespace wolf
{
namespace SE3
{
using namespace Eigen;
 
// JACOBIANS OF SE(3)
 
template <typename D1, typename D2>
inline Matrix<typename D1::Scalar, 3, 3> Q_helper(const MatrixBase<D1>& v, const MatrixBase<D2>& w)
{
    typedef typename D1::Scalar T;
 
    Matrix<T, 3, 3> V  = skew(v);
    Matrix<T, 3, 3> W  = skew(w);
    Matrix<T, 3, 3> WV = W * V, VW = V * W, WW = W * W;
 
    T th_2 = w.squaredNorm();
    T c2, c3, c4;
 
    if (th_2 < T(1e-4))  // th < 1e-2
    {
        T th_4 = th_2 * th_2;
        c2     = T(1. / 6.) - th_2 / T(120.) + th_4 / T(5040.);
        c3     = T(1. / 24.) - th_2 / T(720.) + th_4 / T(40320.);
        c4     = T(1. / 120.) - th_2 / T(2520.) + th_4 / T(120960.);
    }
    else
    {
        T th     = sqrt(th_2);
        T sin_th = sin(th);
        T cos_th = cos(th);
        c2       = (th - sin_th) / (th_2 * th);
        c3       = (th_2 + T(2.0) * cos_th - T(2.0)) / (T(2.0) * th_2 * th_2);
        c4       = (T(2.0) * th - T(3.0) * sin_th + th * cos_th) / (T(2.0) * th_2 * th_2 * th);
    }
 
    Matrix<T, 3, 3> WVW = WV * W;
    Matrix<T, 3, 3> Q =
        T(0.5) * V + c2 * (WV + VW + WVW) + c3 * (WW * V + VW * W - T(3.0) * WVW) + c4 * (WVW * W + WW * V * W);
    return Q;
}
 
template <typename D>
inline Matrix<typename D::Scalar, 6, 6> jac_SE3_left(const MatrixBase<D>& tangent)
{
    MatrixSizeCheck<6, 1>::check(tangent);
    typedef typename D::Scalar T;
 
    Matrix<T, 3, 1> v = tangent.template head<3>();  // linear
    Matrix<T, 3, 1> w = tangent.template tail<3>();  // angular
 
    Matrix<T, 3, 3> Jl = jac_SO3_left(w);
 
    Matrix<T, 6, 6> Jl_SE3;
    Jl_SE3.topLeftCorner(3, 3)     = Jl;
    Jl_SE3.bottomRightCorner(3, 3) = Jl;
    Jl_SE3.topRightCorner(3, 3)    = Q_helper(v, w);
    Jl_SE3.bottomLeftCorner(3, 3).setZero();
    return Jl_SE3;
}
 
template <typename D>
inline Matrix<typename D::Scalar, 6, 6> jac_SE3_right(const MatrixBase<D>& tangent)
{
    return jac_SE3_left(Matrix<typename D::Scalar, 6, 1>(-tangent));
}
 
template <typename D>
inline Matrix<typename D::Scalar, 6, 6> jac_SE3_left_inv(const MatrixBase<D>& tangent)
{
    MatrixSizeCheck<6, 1>::check(tangent);
    typedef typename D::Scalar T;
 
    Matrix<T, 3, 1> v = tangent.template head<3>();  // linear
    Matrix<T, 3, 1> w = tangent.template tail<3>();  // angular
 
    Matrix<T, 3, 3> Jl_inv = jac_SO3_left_inv(w);
 
    Matrix<T, 6, 6> Jl_inv_SE3;
    Jl_inv_SE3.topLeftCorner(3, 3)     = Jl_inv;
    Jl_inv_SE3.bottomRightCorner(3, 3) = Jl_inv;
    Jl_inv_SE3.topRightCorner(3, 3)    = -Jl_inv * Q_helper(v, w) * Jl_inv;
    Jl_inv_SE3.bottomLeftCorner(3, 3).setZero();
    return Jl_inv_SE3;
}
 
template <typename D>
inline Matrix<typename D::Scalar, 6, 6> jac_SE3_right_inv(const MatrixBase<D>& tangent)
{
    return jac_SE3_left_inv(Matrix<typename D::Scalar, 6, 1>(-tangent));
}
 
// IDENTITY
 
template <typename D1, typename D2>
inline void identity(MatrixBase<D1>& p, QuaternionBase<D2>& q)
{
    MatrixSizeCheck<3, 1>::check(p);
    p.setZero();
    q.setIdentity();
}
 
template <typename D1, typename D2>
inline void identity(MatrixBase<D1>& p, MatrixBase<D2>& q)
{
    MatrixSizeCheck<3, 1>::check(p);
    MatrixSizeCheck<4, 1>::check(q);
 
    Map<Quaternion<typename D2::Scalar>> mq(&q(0));
    identity(p, mq);
}
 
template <typename D>
inline void identity(MatrixBase<D>& d)
{
    MatrixSizeCheck<7, 1>::check(d);
 
    Map<Matrix<typename D::Scalar, 3, 1>> p(&d(0));
    Map<Quaternion<typename D::Scalar>>   q(&d(3));
    identity(p, q);
}
 
template <typename T = double>
inline Matrix<T, 7, 1> identity()
{
    Matrix<T, 7, 1> ret;
    identity(ret);
    return ret;
}
 
inline void identity(VectorComposite& v)
{
    v.clear();
    v['P'].resize(3);
    v['O'].resize(4);
    identity(v['P'], v['O']);
}
 
inline VectorComposite identityComposite()
{
    VectorComposite v;
    identity(v);
    return v;
}
 
// INVERSE
 
template <typename D1, typename D2, typename D4, typename D5>
inline void inverse(const MatrixBase<D1>& p, const QuaternionBase<D2>& q, MatrixBase<D4>& ip, QuaternionBase<D5>& iq)
{
    MatrixSizeCheck<3, 1>::check(p);
    MatrixSizeCheck<3, 1>::check(ip);
 
    ip = -(q.conjugate() * p);
    iq = q.conjugate();
}
 
template <typename D1, typename D2, typename D4, typename D5>
inline void inverse(const MatrixBase<D1>& p, const MatrixBase<D2>& q, MatrixBase<D4>& ip, MatrixBase<D5>& iq)
{
    MatrixSizeCheck<4, 1>::check(q);
    MatrixSizeCheck<4, 1>::check(iq);
 
    Map<const Quaternion<typename D2::Scalar>> mq(&q(0));
    Map<Quaternion<typename D5::Scalar>>       miq(&iq(0));
 
    inverse(p, mq, ip, miq);
}
 
template <typename D1, typename D2>
inline void inverse(const MatrixBase<D1>& d, MatrixBase<D2>& id)
{
    MatrixSizeCheck<7, 1>::check(d);
    MatrixSizeCheck<7, 1>::check(id);
 
    Map<const Matrix<typename D1::Scalar, 3, 1>> p(&d(0));
    Map<const Quaternion<typename D1::Scalar>>   q(&d(3));
    Map<Matrix<typename D2::Scalar, 3, 1>>       ip(&id(0));
    Map<Quaternion<typename D2::Scalar>>         iq(&id(3));
 
    inverse(p, q, ip, iq);
}
 
template <typename D>
inline Matrix<typename D::Scalar, 7, 1> inverse(const MatrixBase<D>& d)
{
    Matrix<typename D::Scalar, 7, 1> id;
    inverse(d, id);
    return id;
}
 
inline void inverse(const VectorComposite& v, VectorComposite& c)
{
    c['P'].resize(3);
    c['O'].resize(4);
    inverse(v.at('P'), v.at('O'), c['P'], c['O']);
}
 
inline VectorComposite inverse(const VectorComposite& v)
{
    VectorComposite c;
    inverse(v, c);
    return c;
}
 
// COMPOSE
 
template <typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
inline void compose(const MatrixBase<D1>&     p1,
                    const QuaternionBase<D2>& q1,
                    const MatrixBase<D4>&     p2,
                    const QuaternionBase<D5>& q2,
                    MatrixBase<D7>&           pc,
                    QuaternionBase<D8>&       qc)
{
    MatrixSizeCheck<3, 1>::check(p1);
    MatrixSizeCheck<3, 1>::check(p2);
    MatrixSizeCheck<3, 1>::check(pc);
 
    pc = p1 + q1 * p2;
    qc = q1 * q2;  // q here to avoid possible aliasing between d1 and sum
}
 
template <typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
inline void compose(const MatrixBase<D1>& p1,
                    const MatrixBase<D2>& q1,
                    const MatrixBase<D4>& p2,
                    const MatrixBase<D5>& q2,
                    MatrixBase<D7>&       pc,
                    MatrixBase<D8>&       qc)
{
    MatrixSizeCheck<4, 1>::check(q1);
    MatrixSizeCheck<4, 1>::check(q2);
    MatrixSizeCheck<4, 1>::check(qc);
 
    Map<const Quaternion<typename D2::Scalar>> mq1(&q1(0));
    Map<const Quaternion<typename D5::Scalar>> mq2(&q2(0));
    Map<Quaternion<typename D8::Scalar>>       mqc(&qc(0));
 
    compose(p1, mq1, p2, mq2, pc, mqc);
}
 
template <typename D1, typename D2, typename D3>
inline void compose(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, MatrixBase<D3>& sum)
{
    MatrixSizeCheck<7, 1>::check(d1);
    MatrixSizeCheck<7, 1>::check(d2);
    MatrixSizeCheck<7, 1>::check(sum);
 
    Map<const Matrix<typename D1::Scalar, 3, 1>> p1(&d1(0));
    Map<const Quaternion<typename D1::Scalar>>   q1(&d1(3));
    Map<const Matrix<typename D2::Scalar, 3, 1>> p2(&d2(0));
    Map<const Quaternion<typename D2::Scalar>>   q2(&d2(3));
    Map<Matrix<typename D3::Scalar, 3, 1>>       pc(&sum(0));
    Map<Quaternion<typename D3::Scalar>>         qc(&sum(3));
 
    compose(p1, q1, p2, q2, pc, qc);
}
 
template <typename D1, typename D2>
inline Matrix<typename D1::Scalar, 7, 1> compose(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2)
{
    Matrix<typename D1::Scalar, 7, 1> ret;
    compose(d1, d2, ret);
    return ret;
}
 
template <typename D1, typename D2, typename D3, typename D4, typename D5>
inline void compose(const MatrixBase<D1>& d1,
                    const MatrixBase<D2>& d2,
                    MatrixBase<D3>&       sum,
                    MatrixBase<D4>&       J_sum_d1,
                    MatrixBase<D5>&       J_sum_d2)
{
    MatrixSizeCheck<7, 1>::check(d1);
    MatrixSizeCheck<7, 1>::check(d2);
    MatrixSizeCheck<7, 1>::check(sum);
    MatrixSizeCheck<6, 6>::check(J_sum_d1);
    MatrixSizeCheck<6, 6>::check(J_sum_d2);
 
    // Some useful temporaries
    Matrix<typename D1::Scalar, 3, 3> dR1 = q2R(d1.segment(3, 4));  // q1.matrix(); // First  Delta, DR
    Matrix<typename D2::Scalar, 3, 3> dR2 = q2R(d2.segment(3, 4));  // q2.matrix(); // Second delta, dR
 
    // Jac wrt first delta
    J_sum_d1.setIdentity();                                          // dDp'/dDp = dDv'/dDv = I
    J_sum_d1.block(0, 3, 3, 3).noalias() = -dR1 * skew(d2.head(3));  // dDp'/dDo
    J_sum_d1.block(3, 3, 3, 3)           = dR2.transpose();          // dDo'/dDo
 
    // Jac wrt second delta
    J_sum_d2.setIdentity();            //
    J_sum_d2.block(0, 0, 3, 3) = dR1;  // dDp'/p
    // J_sum_d2.block(3,3,3,3) = Matrix3d::Identity();          // dDo'/ddo = I
 
    // compose deltas -- done here to avoid aliasing when calling with input `d1` and result `sum` referencing the same
    // variable
    compose(d1, d2, sum);
}
 
inline void compose(const VectorComposite& x1, const VectorComposite& x2, VectorComposite& c)
{
    c['P'].resize(3);
    c['O'].resize(4);
    compose(x1.at('P'), x1.at('O'), x2.at('P'), x2.at('O'), c['P'], c['O']);
}
 
inline VectorComposite compose(const VectorComposite& x1, const VectorComposite& x2)
{
    VectorComposite c;
    compose(x1, x2, c);
    return c;
}
 
// inline void compose(const VectorComposite& x1,
//                     const VectorComposite& x2,
//                     VectorComposite&       c,
//                     MatrixComposite&       J_c_x1,
//                     MatrixComposite&       J_c_x2)
// {
//     // Some useful temporaries
//     const auto  R1 = q2R(x1.at('O'));  // q1.matrix(); // make temporary
//     const auto& R2 = q2R(x2.at('O'));  // q2.matrix(); // do not make temporary, only reference
 
//     // Jac wrt first delta
//     J_c_x1.emplace('P', 'P', Matrix3d::Identity());
//     J_c_x1.emplace('P', 'O', -R1 * skew(x2.at('P')));
//     J_c_x1.emplace('O', 'P', Matrix3d::Zero());
//     J_c_x1.emplace('O', 'O', R2.transpose());
 
//     // Jac wrt second delta
//     J_c_x2.emplace('P', 'P', R1);
//     J_c_x2.emplace('P', 'O', Matrix3d::Zero());
//     J_c_x2.emplace('O', 'P', Matrix3d::Zero());
//     J_c_x2.emplace('O', 'O', Matrix3d::Identity());
 
//     // compose deltas -- done here to avoid aliasing when calling with input `x1` and result `c` referencing the
//     same
//     // variable
//     compose(x1, x2, c);
// }
 
// BETWEEN
 
template <typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
inline void between(const MatrixBase<D1>&     p1,
                    const QuaternionBase<D2>& q1,
                    const MatrixBase<D4>&     p2,
                    const QuaternionBase<D5>& q2,
                    MatrixBase<D7>&           p12,
                    QuaternionBase<D8>&       q12)
{
    MatrixSizeCheck<3, 1>::check(p1);
    MatrixSizeCheck<3, 1>::check(p2);
    MatrixSizeCheck<3, 1>::check(p12);
 
    p12 = q1.conjugate() * (p2 - p1);
    q12 = q1.conjugate() * q2;
}
 
template <typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
inline void between(const MatrixBase<D1>& p1,
                    const MatrixBase<D2>& q1,
                    const MatrixBase<D4>& p2,
                    const MatrixBase<D5>& q2,
                    MatrixBase<D7>&       p12,
                    MatrixBase<D8>&       q12)
{
    MatrixSizeCheck<4, 1>::check(q1);
    MatrixSizeCheck<4, 1>::check(q2);
    MatrixSizeCheck<4, 1>::check(q12);
 
    Map<const Quaternion<typename D2::Scalar>> mq1(&q1(0));
    Map<const Quaternion<typename D5::Scalar>> mq2(&q2(0));
    Map<Quaternion<typename D8::Scalar>>       mq12(&q12(0));
 
    between(p1, mq1, p2, mq2, p12, mq12);
}
 
template <typename D1, typename D2, typename D3>
inline void between(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, MatrixBase<D3>& d12)
{
    MatrixSizeCheck<7, 1>::check(d1);
    MatrixSizeCheck<7, 1>::check(d2);
    MatrixSizeCheck<7, 1>::check(d12);
 
    Map<const Matrix<typename D1::Scalar, 3, 1>> p1(&d1(0));
    Map<const Quaternion<typename D1::Scalar>>   q1(&d1(3));
    Map<const Matrix<typename D2::Scalar, 3, 1>> p2(&d2(0));
    Map<const Quaternion<typename D2::Scalar>>   q2(&d2(3));
    Map<Matrix<typename D3::Scalar, 3, 1>>       p12(&d12(0));
    Map<Quaternion<typename D3::Scalar>>         q12(&d12(3));
 
    between(p1, q1, p2, q2, p12, q12);
}
 
template <typename D1, typename D2>
inline Matrix<typename D1::Scalar, 7, 1> between(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2)
{
    Matrix<typename D1::Scalar, 7, 1> d12;
    between(d1, d2, d12);
    return d12;
}
 
inline void between(const VectorComposite& x1, const VectorComposite& x2, VectorComposite& x12)
{
    x12['P'].resize(3);
    x12['O'].resize(4);
    between(x1.at('P'), x1.at('O'), x2.at('P'), x2.at('O'), x12['P'], x12['O']);
}
 
inline VectorComposite between(const VectorComposite& x1, const VectorComposite& x2)
{
    VectorComposite x12;
    between(x1, x2, x12);
    return x12;
}
 
// EXP
 
template <typename D1, typename D2, typename D3, typename D4>
inline void exp(const MatrixBase<D1>& rho, const MatrixBase<D2>& theta, MatrixBase<D3>& p, QuaternionBase<D4>& q)
{
    MatrixSizeCheck<3, 1>::check(rho);
    MatrixSizeCheck<3, 1>::check(theta);
    MatrixSizeCheck<3, 1>::check(p);
 
    // see [1] eqs. (174) and (145)
    p = jac_SO3_left(theta) * rho;
    q = exp_q(theta);
}
 
template <typename D1, typename D2, typename D3, typename D4>
inline void exp(const MatrixBase<D1>& rho, const MatrixBase<D2>& theta, MatrixBase<D3>& p, MatrixBase<D4>& q)
{
    MatrixSizeCheck<4, 1>::check(q);
 
    Map<Quaternion<typename D4::Scalar>> mq(&q(0));
    exp(rho, theta, p, mq);
}
 
template <typename D1, typename D2>
inline void exp(const MatrixBase<D1>& tau, MatrixBase<D2>& delta)
{
    MatrixSizeCheck<6, 1>::check(tau);
    MatrixSizeCheck<7, 1>::check(delta);
 
    Map<const Matrix<typename D1::Scalar, 3, 1>> rho(&tau(0));
    Map<const Matrix<typename D1::Scalar, 3, 1>> theta(&tau(3));
    Map<Matrix<typename D2::Scalar, 3, 1>>       p(&delta(0));
    Map<Quaternion<typename D2::Scalar>>         q(&delta(3));
 
    exp(rho, theta, p, q);
}
 
template <typename D>
inline Matrix<typename D::Scalar, 7, 1> exp(const MatrixBase<D>& tau)
{
    Matrix<typename D::Scalar, 7, 1> delta;
    exp(tau, delta);
    return delta;
}
 
template <typename D1, typename D2, typename D3>
inline void exp(const MatrixBase<D1>& tau, MatrixBase<D2>& delta, MatrixBase<D3>& J_delta_tau)
{
    MatrixSizeCheck<6, 1>::check(tau);
    MatrixSizeCheck<7, 1>::check(delta);
    MatrixSizeCheck<6, 6>::check(J_delta_tau);
 
    typedef typename D1::Scalar T;
 
    Matrix<T, 6, 1> tau_tmp = tau;  // copy (tau and delta may alias)
    Matrix<T, 3, 1> rho     = tau_tmp.template head<3>();
    Matrix<T, 3, 1> theta   = tau_tmp.template tail<3>();
    Matrix<T, 3, 3> Jl      = jac_SO3_left(theta);
    Matrix<T, 3, 1> p       = Jl * rho;
 
    /* Block-wise jacobian (additive position, local rotation), from the left Jacobian of SE(3) [Jl, Q; 0, Jl]:
     *   J_p_rho     = Jl(theta)
     *   J_p_theta   = Q(rho, theta) - [p]x * Jl(theta)
     *   J_o_rho     = 0
     *   J_o_theta   = Jr(theta)
     */
    J_delta_tau.setZero();
    J_delta_tau.template topLeftCorner<3, 3>()     = Jl;
    J_delta_tau.template topRightCorner<3, 3>()    = Q_helper(rho, theta) - skew(p) * Jl;
    J_delta_tau.template bottomRightCorner<3, 3>() = jac_SO3_right(theta);
 
    exp(tau_tmp, delta);
}
 
inline void exp(const VectorComposite& tau, VectorComposite& delta)
{
    // temporaries: tangent and manifold 'O' have different sizes (tau and delta may be the same object)
    Vector3d p;
    Vector4d q;
    exp(tau.at('P'), tau.at('O'), p, q);
    delta['P'] = p;
    delta['O'] = q;
}
 
inline VectorComposite exp(const VectorComposite& tau)
{
    VectorComposite delta;
    exp(tau, delta);
    return delta;
}
 
// LOG
 
template <typename D1, typename D2, typename D3, typename D4>
inline void log(const MatrixBase<D1>& p, const QuaternionBase<D2>& q, MatrixBase<D3>& rho, MatrixBase<D4>& theta)
{
    MatrixSizeCheck<3, 1>::check(p);
    MatrixSizeCheck<3, 1>::check(rho);
    MatrixSizeCheck<3, 1>::check(theta);
 
    // see [1] eqs. (173) and (146)
    Matrix<typename D2::Scalar, 3, 1> th = log_q(q);
    rho                                  = jac_SO3_left_inv(th) * p;
    theta                                = th;
}
 
template <typename D1, typename D2, typename D3, typename D4>
inline void log(const MatrixBase<D1>& p, const MatrixBase<D2>& q, MatrixBase<D3>& rho, MatrixBase<D4>& theta)
{
    MatrixSizeCheck<4, 1>::check(q);
 
    Map<const Quaternion<typename D2::Scalar>> mq(&q(0));
    log(p, mq, rho, theta);
}
 
template <typename D1, typename D2>
inline void log(const MatrixBase<D1>& delta, MatrixBase<D2>& tau)
{
    MatrixSizeCheck<7, 1>::check(delta);
    MatrixSizeCheck<6, 1>::check(tau);
 
    Map<const Matrix<typename D1::Scalar, 3, 1>> p(&delta(0));
    Map<const Quaternion<typename D1::Scalar>>   q(&delta(3));
    Map<Matrix<typename D2::Scalar, 3, 1>>       rho(&tau(0));
    Map<Matrix<typename D2::Scalar, 3, 1>>       theta(&tau(3));
 
    log(p, q, rho, theta);
}
 
template <typename D>
inline Matrix<typename D::Scalar, 6, 1> log(const MatrixBase<D>& delta)
{
    Matrix<typename D::Scalar, 6, 1> tau;
    log(delta, tau);
    return tau;
}
 
template <typename D1, typename D2, typename D3>
inline void log(const MatrixBase<D1>& delta, MatrixBase<D2>& tau, MatrixBase<D3>& J_tau_delta)
{
    MatrixSizeCheck<7, 1>::check(delta);
    MatrixSizeCheck<6, 1>::check(tau);
    MatrixSizeCheck<6, 6>::check(J_tau_delta);
 
    typedef typename D1::Scalar T;
 
    Matrix<T, 6, 1> tau_tmp;
    log(delta, tau_tmp);
    Matrix<T, 3, 1> rho   = tau_tmp.template head<3>();
    Matrix<T, 3, 1> theta = tau_tmp.template tail<3>();
    Matrix<T, 3, 1> p     = delta.template head<3>();
 
    /* Block-wise jacobian (additive position, local rotation): inverse of the jacobian of exp()
     *   J_exp = [A, B; 0, C] with A = Jl(theta), B = Q(rho, theta) - [p]x * Jl(theta), C = Jr(theta)
     *   J_log = [A.inv, -A.inv * B * C.inv; 0, C.inv]
     */
    Matrix<T, 3, 3> Jl_inv = jac_SO3_left_inv(theta);
    Matrix<T, 3, 3> Jr_inv = jac_SO3_right_inv(theta);
    Matrix<T, 3, 3> B      = Q_helper(rho, theta) - skew(p) * jac_SO3_left(theta);
 
    J_tau_delta.setZero();
    J_tau_delta.template topLeftCorner<3, 3>()     = Jl_inv;
    J_tau_delta.template topRightCorner<3, 3>()    = -Jl_inv * B * Jr_inv;
    J_tau_delta.template bottomRightCorner<3, 3>() = Jr_inv;
 
    tau = tau_tmp;
}
 
inline void log(const VectorComposite& delta, VectorComposite& tau)
{
    // temporaries: tangent and manifold 'O' have different sizes (tau and delta may be the same object)
    Vector3d rho, theta;
    log(delta.at('P'), delta.at('O'), rho, theta);
    tau['P'] = rho;
    tau['O'] = theta;
}
 
inline VectorComposite log(const VectorComposite& delta)
{
    VectorComposite tau;
    log(delta, tau);
    return tau;
}
 
// PLUS (block-wise)
 
template <typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
inline void plus(const MatrixBase<D1>&     p1,
                 const QuaternionBase<D2>& q1,
                 const MatrixBase<D4>&     p2,
                 const MatrixBase<D5>&     o2,
                 MatrixBase<D7>&           plus_p,
                 QuaternionBase<D8>&       plus_q)
{
    MatrixSizeCheck<3, 1>::check(p1);
    MatrixSizeCheck<3, 1>::check(p2);
    MatrixSizeCheck<3, 1>::check(o2);
    MatrixSizeCheck<3, 1>::check(plus_p);
 
    plus_p = p1 + p2;
    plus_q = q1 * exp_q(o2);
}
 
template <typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
inline void plus(const MatrixBase<D1>& p1,
                 const MatrixBase<D2>& q1,
                 const MatrixBase<D4>& p2,
                 const MatrixBase<D5>& o2,
                 MatrixBase<D7>&       plus_p,
                 MatrixBase<D8>&       plus_q)
{
    MatrixSizeCheck<4, 1>::check(q1);
    MatrixSizeCheck<4, 1>::check(plus_q);
 
    Map<const Quaternion<typename D2::Scalar>> q1m(&q1(0));
    Map<Quaternion<typename D8::Scalar>>       plus_qm(&plus_q(0));
 
    plus(p1, q1m, p2, o2, plus_p, plus_qm);
}
 
template <typename D1, typename D2, typename D3>
inline void plus(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, MatrixBase<D3>& d_plus)
{
    MatrixSizeCheck<7, 1>::check(d1);
    MatrixSizeCheck<6, 1>::check(d2);
    MatrixSizeCheck<7, 1>::check(d_plus);
 
    Map<const Matrix<typename D1::Scalar, 3, 1>> p1(&d1(0));
    Map<const Quaternion<typename D1::Scalar>>   q1(&d1(3));
    Map<const Matrix<typename D2::Scalar, 3, 1>> p2(&d2(0));
    Map<const Matrix<typename D2::Scalar, 3, 1>> o2(&d2(3));
    Map<Matrix<typename D3::Scalar, 3, 1>>       p_p(&d_plus(0));
    Map<Quaternion<typename D3::Scalar>>         q_p(&d_plus(3));
 
    plus(p1, q1, p2, o2, p_p, q_p);
}
 
template <typename D1, typename D2>
inline Matrix<typename D1::Scalar, 7, 1> plus(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2)
{
    Matrix<typename D1::Scalar, 7, 1> d_plus;
    plus(d1, d2, d_plus);
    return d_plus;
}
 
inline void plus(const VectorComposite& x, const VectorComposite& tau, VectorComposite& res)
{
    res['P'].resize(3);
    res['O'].resize(4);
    plus(x.at('P'), x.at('O'), tau.at('P'), tau.at('O'), res['P'], res['O']);
}
 
inline VectorComposite plus(const VectorComposite& x, const VectorComposite& tau)
{
    VectorComposite res;
    plus(x, tau, res);
    return res;
}
 
// MINUS (block-wise)
 
template <typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
inline void minus(const MatrixBase<D1>&     p1,
                  const QuaternionBase<D2>& q1,
                  const MatrixBase<D4>&     p2,
                  const QuaternionBase<D5>& q2,
                  MatrixBase<D7>&           pd,
                  MatrixBase<D8>&           od)
{
    MatrixSizeCheck<3, 1>::check(p1);
    MatrixSizeCheck<3, 1>::check(p2);
    MatrixSizeCheck<3, 1>::check(pd);
    MatrixSizeCheck<3, 1>::check(od);
 
    pd = p2 - p1;
    od = log_q(q1.conjugate() * q2);
}
 
template <typename D1, typename D2, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9>
inline void minus(const MatrixBase<D1>&     p1,
                  const QuaternionBase<D2>& q1,
                  const MatrixBase<D4>&     p2,
                  const QuaternionBase<D5>& q2,
                  MatrixBase<D6>&           pd,
                  MatrixBase<D7>&           od,
                  MatrixBase<D8>&           J_do_q1,
                  MatrixBase<D9>&           J_do_q2)
{
    minus(p1, q1, p2, q2, pd, od);
 
    J_do_q1 = -jac_SO3_left_inv(od);
    J_do_q2 = jac_SO3_right_inv(od);
}
 
template <typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
inline void minus(const MatrixBase<D1>& p1,
                  const MatrixBase<D2>& q1,
                  const MatrixBase<D4>& p2,
                  const MatrixBase<D5>& q2,
                  MatrixBase<D7>&       pd,
                  MatrixBase<D8>&       od)
{
    MatrixSizeCheck<4, 1>::check(q1);
    MatrixSizeCheck<4, 1>::check(q2);
 
    Map<const Quaternion<typename D2::Scalar>> mq1(&q1(0));
    Map<const Quaternion<typename D5::Scalar>> mq2(&q2(0));
 
    minus(p1, mq1, p2, mq2, pd, od);
}
 
template <typename D1, typename D2, typename D3>
inline void minus(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, MatrixBase<D3>& err)
{
    MatrixSizeCheck<7, 1>::check(d1);
    MatrixSizeCheck<7, 1>::check(d2);
    MatrixSizeCheck<6, 1>::check(err);
 
    Map<const Matrix<typename D1::Scalar, 3, 1>> p1(&d1(0));
    Map<const Quaternion<typename D1::Scalar>>   q1(&d1(3));
    Map<const Matrix<typename D2::Scalar, 3, 1>> p2(&d2(0));
    Map<const Quaternion<typename D2::Scalar>>   q2(&d2(3));
    Map<Matrix<typename D3::Scalar, 3, 1>>       pd(&err(0));
    Map<Matrix<typename D3::Scalar, 3, 1>>       od(&err(3));
 
    minus(p1, q1, p2, q2, pd, od);
}
 
template <typename D1, typename D2, typename D3, typename D4, typename D5>
inline void minus(const MatrixBase<D1>& d1,
                  const MatrixBase<D2>& d2,
                  MatrixBase<D3>&       dif,
                  MatrixBase<D4>&       J_diff_d1,
                  MatrixBase<D5>&       J_diff_d2)
{
    MatrixSizeCheck<7, 1>::check(d1);
    MatrixSizeCheck<7, 1>::check(d2);
    MatrixSizeCheck<6, 1>::check(dif);
    MatrixSizeCheck<6, 6>::check(J_diff_d1);
    MatrixSizeCheck<6, 6>::check(J_diff_d2);
 
    Map<const Matrix<typename D1::Scalar, 3, 1>> p1(&d1(0));
    Map<const Quaternion<typename D1::Scalar>>   q1(&d1(3));
    Map<const Matrix<typename D2::Scalar, 3, 1>> p2(&d2(0));
    Map<const Quaternion<typename D2::Scalar>>   q2(&d2(3));
    Map<Matrix<typename D3::Scalar, 3, 1>>       pd(&dif(0));
    Map<Matrix<typename D3::Scalar, 3, 1>>       od(&dif(3));
 
    Matrix<typename D4::Scalar, 3, 3> J_do_q1, J_do_q2;
 
    minus(p1, q1, p2, q2, pd, od, J_do_q1, J_do_q2);
 
    /* d = minus(d1, d2) is
     *   pd = p2 - p1
     *   od = Log(q1.conj * q2)
     *
     * With trivial Jacobians for p and dv, and:
     *   J_do_q1 = - J_l_inv(theta)
     *   J_do_q2 =   J_r_inv(theta)
     */
    J_diff_d1                   = -Matrix<typename D4::Scalar, 6, 6>::Identity();  // d(p2  - p1) / d(p1) = - Identity
    J_diff_d1.block(3, 3, 3, 3) = J_do_q1;  // d(R1.tr*R2) / d(R1) = - J_l_inv(theta)
 
    J_diff_d2.setIdentity(6, 6);            // d(R1.tr*R2) / d(R2) =   Identity
    J_diff_d2.block(3, 3, 3, 3) = J_do_q2;  // d(R1.tr*R2) / d(R1) =   J_r_inv(theta)
}
 
template <typename D1, typename D2>
inline Matrix<typename D1::Scalar, 6, 1> minus(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2)
{
    Matrix<typename D1::Scalar, 6, 1> ret;
    minus(d1, d2, ret);
    return ret;
}
 
inline void minus(const VectorComposite& x1, const VectorComposite& x2, VectorComposite& res)
{
    // temporaries: tangent and manifold 'O' have different sizes (res and x1 or x2 may be the same object)
    Vector3d pd, od;
    minus(x1.at('P'), x1.at('O'), x2.at('P'), x2.at('O'), pd, od);
    res['P'] = pd;
    res['O'] = od;
}
 
inline VectorComposite minus(const VectorComposite& x1, const VectorComposite& x2)
{
    VectorComposite res;
    minus(x1, x2, res);
    return res;
}
 
// INTERPOLATE
 
template <typename D1, typename D2, typename D3>
inline void interpolate(const MatrixBase<D1>&      d1,
                        const MatrixBase<D2>&      d2,
                        const typename D1::Scalar& t,
                        MatrixBase<D3>&            ret)
{
    MatrixSizeCheck<7, 1>::check(d1);
    MatrixSizeCheck<7, 1>::check(d2);
    MatrixSizeCheck<7, 1>::check(ret);
 
    Matrix<typename D1::Scalar, 7, 1> dd = between(d1, d2);
 
    Matrix<typename D1::Scalar, 6, 1> tau = t * log(dd);
 
    ret = compose(d1, exp(tau));
}
 
template <typename D1, typename D2>
inline Matrix<typename D1::Scalar, 7, 1> interpolate(const MatrixBase<D1>&      d1,
                                                     const MatrixBase<D2>&      d2,
                                                     const typename D1::Scalar& t)
{
    Matrix<typename D1::Scalar, 7, 1> ret;
    interpolate(d1, d2, t, ret);
    return ret;
}
 
inline void interpolate(const VectorComposite& x1, const VectorComposite& x2, const double& t, VectorComposite& res)
{
    VectorComposite tau = log(between(x1, x2));
    tau['P'] *= t;
    tau['O'] *= t;
    compose(x1, exp(tau), res);
}
 
inline VectorComposite interpolate(const VectorComposite& x1, const VectorComposite& x2, const double& t)
{
    VectorComposite res;
    interpolate(x1, x2, t, res);
    return res;
}
 
// HOMOGENEOUS MATRIX
 
template <typename D1, typename D2>
inline void toSE3(const MatrixBase<D1>& pose, MatrixBase<D2>& SE3)
{
    MatrixSizeCheck<7, 1>::check(pose);
    MatrixSizeCheck<4, 4>::check(SE3);
 
    typedef typename D1::Scalar T;
 
    SE3.template block<3, 1>(0, 3) = pose.template head<3>();
    SE3.template block<3, 3>(0, 0) = q2R(pose.template tail<4>());
    SE3.template block<1, 3>(3, 0).setZero();
    SE3(3, 3) = (T)1.0;
}
 
template <typename D>
inline Matrix<typename D::Scalar, 4, 4> toSE3(const MatrixBase<D>& pose)
{
    Matrix<typename D::Scalar, 4, 4> SE3;
    toSE3(pose, SE3);
    return SE3;
}
 
}  // namespace SE3
}  // namespace wolf